/**
  ******************************************************************************
  * @file           : rudder_wheel_kinematic_drv.cpp
  * @author         : Tessia
  * @brief          : None
  * @attention      : None
  * @date           : 2024/2/23
  ******************************************************************************
  */



#include "rudder_wheel_kinematic_drv.h"

extern uint8_t is_on_slope;
extern INS_t INS;

float pitch_filtered = 0;
float roll_filtered = 0;

rudder_wheel::rudder_wheel(float speed_max_, float omega_max_, float rudder2center_x_, float rudder2center_y_):
              motionParameter{}, speed_max(speed_max_), omega_max(omega_max_),
              rudder2center_x(rudder2center_x_), rudder2center_y(rudder2center_y_),is_self_lock(0),
              rudderRound{}, rudderRound_last{}, wheelSpeed{}  {
    alpha = atan(rudder2center_y / rudder2center_x);
}

void rudder_wheel::get_motion_parameter(float vx, float vy, float omega){
    motionParameter.vx = vx * speed_max;
    motionParameter.vy = vy * speed_max;
    motionParameter.omega = omega * omega_max; //逆时针为正
}

void rudder_wheel::rudder_wheel_cal(){

    rudderRound_last.left_front_rudder = rudderRound.left_front_rudder;
    rudderRound_last.right_front_rudder = rudderRound.right_front_rudder;
    rudderRound_last.left_back_rudder = rudderRound.left_back_rudder;
    rudderRound_last.right_back_rudder = rudderRound.right_back_rudder;

    if(abs(motionParameter.vx) > 1e-2 || abs(motionParameter.vy) > 1e-2 || abs(motionParameter.omega) > 1e-2) {

        is_self_lock = 0;
        rudderRound.left_front_rudder = atan2(motionParameter.vy - arm_cos_f32(alpha) * motionParameter.omega,
                                              motionParameter.vx - arm_sin_f32(alpha) * motionParameter.omega);
        rudderRound.right_front_rudder = atan2(motionParameter.vy + arm_cos_f32(alpha) * motionParameter.omega,
                                               motionParameter.vx - arm_sin_f32(alpha) * motionParameter.omega);
        rudderRound.left_back_rudder = atan2(motionParameter.vy - arm_cos_f32(alpha) * motionParameter.omega,
                                             motionParameter.vx + arm_sin_f32(alpha) * motionParameter.omega);
        rudderRound.right_back_rudder = atan2(motionParameter.vy + arm_cos_f32(alpha) * motionParameter.omega,
                                               motionParameter.vx + arm_sin_f32(alpha) * motionParameter.omega);


    }else{
        is_self_lock = 1;
        rudderRound.left_front_rudder = atan2(- arm_cos_f32(alpha), - arm_sin_f32(alpha));
        rudderRound.right_front_rudder = atan2(arm_cos_f32(alpha), - arm_sin_f32(alpha));
        rudderRound.left_back_rudder = atan2(- arm_cos_f32(alpha), arm_sin_f32(alpha));
        rudderRound.right_back_rudder = atan2(arm_cos_f32(alpha), arm_sin_f32(alpha));

    }

    if(is_on_slope == 0 || is_self_lock == 1){
        pitch_filtered = 0;
        roll_filtered = 0;
        wheelSpeed.left_front_wheel = pow((pow(motionParameter.vy - arm_cos_f32(alpha) * motionParameter.omega, 2) +
                                           pow(motionParameter.vx - arm_sin_f32(alpha) * motionParameter.omega, 2)), 0.5);
        wheelSpeed.right_front_wheel = pow((pow(motionParameter.vy + arm_cos_f32(alpha) * motionParameter.omega, 2) +
                                            pow(motionParameter.vx - arm_sin_f32(alpha) * motionParameter.omega, 2)), 0.5);
        wheelSpeed.left_back_wheel = pow((pow(motionParameter.vy - arm_cos_f32(alpha) * motionParameter.omega, 2) +
                                          pow(motionParameter.vx + arm_sin_f32(alpha) * motionParameter.omega, 2)), 0.5);
        wheelSpeed.right_back_wheel = pow((pow(motionParameter.vy + arm_cos_f32(alpha) * motionParameter.omega, 2) +
                                           pow(motionParameter.vx + arm_sin_f32(alpha) * motionParameter.omega, 2)), 0.5);
    }else{
        low_pass(pitch_filtered, INS.Pitch, GYRO_FILTERED_ALPHA);
        low_pass(roll_filtered, INS.Roll, GYRO_FILTERED_ALPHA);
        wheelSpeed.left_front_wheel = pow((pow(motionParameter.vy - arm_cos_f32(alpha) * motionParameter.omega, 2) +
                                           pow(motionParameter.vx - arm_sin_f32(alpha) * motionParameter.omega, 2)), 0.5) + pitch_filtered / SLOPE_SCALING_FACTOR - roll_filtered / SLOPE_SCALING_FACTOR;
        wheelSpeed.right_front_wheel = pow((pow(motionParameter.vy + arm_cos_f32(alpha) * motionParameter.omega, 2) +
                                            pow(motionParameter.vx - arm_sin_f32(alpha) * motionParameter.omega, 2)), 0.5) + pitch_filtered / SLOPE_SCALING_FACTOR + roll_filtered / SLOPE_SCALING_FACTOR;
        wheelSpeed.left_back_wheel = pow((pow(motionParameter.vy - arm_cos_f32(alpha) * motionParameter.omega, 2) +
                                          pow(motionParameter.vx + arm_sin_f32(alpha) * motionParameter.omega, 2)), 0.5) - pitch_filtered / SLOPE_SCALING_FACTOR - roll_filtered / SLOPE_SCALING_FACTOR;
        wheelSpeed.right_back_wheel = pow((pow(motionParameter.vy + arm_cos_f32(alpha) * motionParameter.omega, 2) +
                                           pow(motionParameter.vx + arm_sin_f32(alpha) * motionParameter.omega, 2)), 0.5) - pitch_filtered / SLOPE_SCALING_FACTOR + roll_filtered / SLOPE_SCALING_FACTOR;

    }



}
